#include "PID.h"

void new_PID(PID_Class *mpid,PID_type* Input,PID_type* Output,PID_type* Setpoint,PID_type Kp,PID_type Ki,PID_type Kd,bool POn, bool ControllerDirection)
{
    mpid->myOutput = Output;
    mpid->myInput = Input;
    mpid->mySetpoint = Setpoint;
    mpid->inAuto = false;

    PID_SetOutputLimits(mpid,0,255);				//default output limit corresponds to
												//the arduino pwm limits

    mpid->SampleTime = 100;							//default Controller Sample Time is 0.1 seconds

    PID_SetControllerDirection(mpid,ControllerDirection);
    PID_SetTunings(mpid,Kp, Ki, Kd, POn);

    mpid->lastTime = PIDmillis()-(mpid->SampleTime);
}

void PID_SetTunings(PID_Class *mpid,PID_type Kp, PID_type Ki, PID_type Kd, bool POn)
{
		PID_type SampleTimeInSec = 0;
   if (Kp<0 || Ki<0 || Kd<0) return;

   //mpid->pOn = POn;
   mpid->pOnE = POn;

   mpid->dispKp = Kp; mpid->dispKi = Ki; mpid->dispKd = Kd;

   SampleTimeInSec = ((PID_type)mpid->SampleTime)/1000;
   mpid->kp = Kp;
   mpid->ki = Ki * SampleTimeInSec;
   mpid->kd = Kd / SampleTimeInSec;

  if(mpid->controllerDirection ==REVERSE)
   {
      mpid->kp = (0 - mpid->kp);
      mpid->ki = (0 - mpid->kp);
      mpid->kd = (0 - mpid->kp);
   }
}
void PID_SetSampleTime(PID_Class *mpid,Timetick NewSampleTime)
{
   if (NewSampleTime > 0)
   {
      PID_type ratio  = (PID_type)NewSampleTime
                      / (PID_type)mpid->SampleTime;
      mpid->ki *= ratio;
      mpid->kd /= ratio;
      mpid->SampleTime = (Timetick)NewSampleTime;
   }
}
void PID_SetOutputLimits(PID_Class *mpid,PID_type Min, PID_type Max)
{
   if(Min >= Max) return;
   mpid->outMin = Min;
   mpid->outMax = Max;

   if(mpid->inAuto)
   {
	   if(*(mpid->myOutput) > (mpid->outMax)) *(mpid->myOutput) = (mpid->outMax);
	   else if(*(mpid->myOutput) < (mpid->outMin)) *(mpid->myOutput) = (mpid->outMin);

	   if((mpid->outputSum) > mpid->outMax) (mpid->outputSum)= mpid->outMax;
	   else if((mpid->outputSum) < mpid->outMin) mpid->outputSum= mpid->outMin;
   }
}

void PID_SetMode(PID_Class *mpid,bool Mode)
{
    bool newAuto = (Mode == AUTOMATIC);
    if(newAuto && !(mpid->inAuto))
    {  
        PID_Initialize(mpid);
    }
    mpid->inAuto = newAuto;
}
void PID_Initialize(PID_Class *mpid)
{
   mpid->outputSum = *(mpid->myOutput);
   mpid->lastInput = *(mpid->myInput);
   if(mpid->outputSum > mpid->outMax) mpid->outputSum = mpid->outMax;
   else if(mpid->outputSum < mpid->outMin) mpid->outputSum = mpid->outMin;
}
void PID_SetControllerDirection(PID_Class *mpid,bool Direction)
{
   if(mpid->inAuto && Direction !=mpid->controllerDirection)
   {
	    mpid->kp = (0 - mpid->kp);
      mpid->ki = (0 - mpid->ki);
      mpid->kd = (0 - mpid->kd);
   }
   mpid->controllerDirection = Direction;
}

bool PID_Compute(PID_Class *mpid)
{
	 PID_type output;
	Timetick now;
	Timetick timeChange;
   if(!(mpid->inAuto))
	 {
		 return false;
	 }
	 now = PIDmillis();
   timeChange = (now - mpid->lastTime);
   if(timeChange>=(mpid->SampleTime))
   {
      /*Compute all the working error variables*/
      PID_type input = *(mpid->myInput);
      PID_type error = *(mpid->mySetpoint) - input;
      PID_type dInput = (input - mpid->lastInput);
      mpid->outputSum+= (mpid->ki * error);

      /*Add Proportional on Measurement, if P_ON_M is specified*/
      if(!(mpid->pOnE)) (mpid->outputSum)-= (mpid->kp) * dInput;

      if((mpid->outputSum) > (mpid->outMax)) (mpid->outputSum)= (mpid->outMax);
      else if((mpid->outputSum) < (mpid->outMin)) (mpid->outputSum)= (mpid->outMin);

      /*Add Proportional on Error, if P_ON_E is specified*/
      if(mpid->pOnE) output = mpid->kp * error;
      else output = 0;

      /*Compute Rest of PID Output*/
      output += mpid->outputSum - mpid->kd * dInput;

	    if(output > mpid->outMax) output = mpid->outMax;
      else if(output < mpid->outMin) output = mpid->outMin;
	    *(mpid->myOutput) = output;

      /*Remember some variables for next time*/
      mpid->lastInput = input;
      mpid->lastTime = now;
	    return true;
   }
   else return false;
}